#include <linux/rbtree_augmented.h>

static inline void
rb_insert_augmented(struct rb_node *node, struct rb_root *root,
                    const struct rb_augment_callbacks *augment)
{
    __rb_insert_augmented(node, root, augment->rotate);
}

void rb_insert_augmented_cached(struct rb_node *node,
                                struct rb_root_cached *root, bool newleft,
                                const struct rb_augment_callbacks *augment)
{
    if (newleft)
        root->rb_leftmost = node;
    rb_insert_augmented(node, &root->rb_root, augment);
}

struct rb_node *rb_add_augmented_cached(struct rb_node *node,
                                        struct rb_root_cached *tree,
                                        bool (*less)(struct rb_node *, const struct rb_node *),
                                        const struct rb_augment_callbacks *augment)
{
    struct rb_node **link = &tree->rb_root.rb_node;
    struct rb_node *parent = NULL;
    bool leftmost = true;

    while (*link)
    {
        parent = *link;
        if (less(node, parent))
        {
            link = &parent->rb_left;
        }
        else
        {
            link = &parent->rb_right;
            leftmost = false;
        }
    }

    rb_link_node(node, parent, link);
    augment->propagate(parent, NULL); /* suboptimal */
    rb_insert_augmented_cached(node, tree, leftmost, augment);

    return leftmost ? node : NULL;
}

struct rb_node *__rb_erase_augmented(struct rb_node *node, struct rb_root *root,
                                     const struct rb_augment_callbacks *augment)
{
    struct rb_node *child = node->rb_right;
    struct rb_node *tmp = node->rb_left;
    struct rb_node *parent, *rebalance;
    unsigned long pc;

    if (!tmp)
    {
        /*
         * Case 1: node to erase has no more than 1 child (easy!)
         *
         * Note that if there is one child it must be red due to 5)
         * and node must be black due to 4). We adjust colors locally
         * so as to bypass __rb_erase_color() later on.
         */
        pc = node->__rb_parent_color;
        parent = __rb_parent(pc);
        __rb_change_child(node, child, parent, root);
        if (child)
        {
            child->__rb_parent_color = pc;
            rebalance = NULL;
        }
        else
            rebalance = __rb_is_black(pc) ? parent : NULL;
        tmp = parent;
    }
    else if (!child)
    {
        /* Still case 1, but this time the child is node->rb_left */
        tmp->__rb_parent_color = pc = node->__rb_parent_color;
        parent = __rb_parent(pc);
        __rb_change_child(node, tmp, parent, root);
        rebalance = NULL;
        tmp = parent;
    }
    else
    {
        struct rb_node *successor = child, *child2;

        tmp = child->rb_left;
        if (!tmp)
        {
            /*
             * Case 2: node's successor is its right child
             *
             *    (n)          (s)
             *    / \          / \
             *  (x) (s)  ->  (x) (c)
             *        \
             *        (c)
             */
            parent = successor;
            child2 = successor->rb_right;

            augment->copy(node, successor);
        }
        else
        {
            /*
             * Case 3: node's successor is leftmost under
             * node's right child subtree
             *
             *    (n)          (s)
             *    / \          / \
             *  (x) (y)  ->  (x) (y)
             *      /            /
             *    (p)          (p)
             *    /            /
             *  (s)          (c)
             *    \
             *    (c)
             */
            do
            {
                parent = successor;
                successor = tmp;
                tmp = tmp->rb_left;
            } while (tmp);
            child2 = successor->rb_right;
            WRITE_ONCE(parent->rb_left, child2);
            WRITE_ONCE(successor->rb_right, child);
            rb_set_parent(child, successor);

            augment->copy(node, successor);
            augment->propagate(parent, successor);
        }

        tmp = node->rb_left;
        WRITE_ONCE(successor->rb_left, tmp);
        rb_set_parent(tmp, successor);

        pc = node->__rb_parent_color;
        tmp = __rb_parent(pc);
        __rb_change_child(node, successor, tmp, root);

        if (child2)
        {
            rb_set_parent_color(child2, parent, RB_BLACK);
            rebalance = NULL;
        }
        else
        {
            rebalance = rb_is_black(successor) ? parent : NULL;
        }
        successor->__rb_parent_color = pc;
        tmp = successor;
    }

    augment->propagate(tmp, NULL);
    return rebalance;
}

void __rb_change_child(struct rb_node *old, struct rb_node *new,
                       struct rb_node *parent, struct rb_root *root)
{
    if (parent)
    {
        if (parent->rb_left == old)
            WRITE_ONCE(parent->rb_left, new);
        else
            WRITE_ONCE(parent->rb_right, new);
    }
    else
        WRITE_ONCE(root->rb_node, new);
}
